#include "glut.h"
#include <math.h>

int windowWidth = 1024;
int windowHeight = 768;

double eye[] = {0,0,1};
double center[] = {0,0,0};
double up[] = {0,1,0};

float ElbowJointAngle = 0;
float WristJointAngle = 0;
float FingerJointAngle = 0;

float AspectRatio = float(windowWidth) / float(windowHeight);

//global functions
void display();
void KeyboardEvent(unsigned char key, int x, int y);
void specialKeys(int key, int x, int y);


void main()
{	
	glutInitWindowSize(windowWidth, windowHeight);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB);
	glutCreateWindow("Assignment 1");

	glMatrixMode(GL_PROJECTION);

	gluPerspective(60, AspectRatio, 0, 100);
	

	glutKeyboardFunc(KeyboardEvent);

	glutDisplayFunc(display);
	glutSpecialFunc(specialKeys);	

	glutMainLoop();

}



void display()
{
	glClearColor(0.0, 0.0, 0.0, 0.0);
	glClear(GL_COLOR_BUFFER_BIT);
	
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();	

	gluLookAt(eye[0],eye[1],eye[2],center[0],center[1],center[2],up[0],up[1],up[2]);

	glColor3f(1,0,0);
	glPushMatrix();
	//Origin
	glutWireCube(0.001);
	glPopMatrix();

	glColor3f(0,0,1);
	//Start Translations Arm
		glPushMatrix();
			
			//Lower Arm
			glPushMatrix();
			glScalef(0.25,0.25,1);
			glutWireCube(0.3);
			glPopMatrix();

			//Upper Arm
			glPushMatrix();
			glTranslatef(0,0,0.34);

				//Rotate Arm
				glTranslatef(0,0,-.16);
				glRotatef(ElbowJointAngle,1,0,0);
				glTranslatef(0,0,.16);

			glScalef(0.25,0.25,1);
			glutWireCube(0.3);
			glPopMatrix();
		

		glPopMatrix();


//Fingers
glPushMatrix();
//Rotate Wrist



		glPushMatrix();

				//Stick with Arm Rotation
				glTranslatef(0,0,+0.19);
				glRotatef(ElbowJointAngle,1,0,0);
				glTranslatef(0,0,-0.19);
					glTranslatef(0,0,0.34+0.15);
					glRotatef(WristJointAngle,1,0,0);
					glTranslatef(0,0,-0.34-0.15);


			glPushMatrix();
				glTranslatef(-0.025,0.025,0.34+0.19);
				glScalef(0.05,0.05,0.2);
				glutWireCube(0.3);
				glPopMatrix();

			glPushMatrix();
				glTranslatef(0,0.025,0.34+0.19);
				glScalef(0.05,0.05,0.2);
				glutWireCube(0.3);
				glPopMatrix();

			glPushMatrix();
				glTranslatef(0.025,0.025,0.34+0.19);
				glScalef(0.05,0.05,0.2);
				glutWireCube(0.3);
				glPopMatrix();

			glPushMatrix();
				glTranslatef(0,-0.025,0.34+0.19);
				glScalef(0.05,0.05,0.2);
				glutWireCube(0.3);
				glPopMatrix();

		glPopMatrix();

	
	    glPushMatrix();
				//Fingers Part Two
				//Stick with Arm Rotation
				glTranslatef(0,0,+0.19);
				glRotatef(ElbowJointAngle,1,0,0);
				glTranslatef(0,0,-0.19);
				glTranslatef(0,0,0.34+0.15);
					glRotatef(WristJointAngle,1,0,0);
					glTranslatef(0,0,-0.34-0.15);


		glPushMatrix();
					glTranslatef(0,0,+0.34+0.232);
					glRotatef(FingerJointAngle,1,0,0);
					glTranslatef(0,0,-0.34-0.232);


	

			glPushMatrix();
				glTranslatef(-0.025,0.025,0.34+0.19+0.075);
				glScalef(0.05,0.05,0.2);
				glutWireCube(0.3);
				glPopMatrix();

			glPushMatrix();
				glTranslatef(0,0.025,0.34+0.19+0.075);
				glScalef(0.05,0.05,0.2);
				glutWireCube(0.3);
			glPopMatrix();

			glPushMatrix();
				glTranslatef(0.025,0.025,0.34+0.19+0.075);
				glScalef(0.05,0.05,0.2);
				glutWireCube(0.3);
			glPopMatrix();


		glPopMatrix();

					glTranslatef(0,0,+0.34+0.232);
					glRotatef(-FingerJointAngle,1,0,0);
					glTranslatef(0,0,-0.34-0.232);


			glPushMatrix();
				glTranslatef(0,-0.025,0.34+0.19+0.075);
				glScalef(0.05,0.05,0.2);
				glutWireCube(0.3);
			glPopMatrix();
								
		glPopMatrix();

glPopMatrix();


	glFlush ();
	glutSwapBuffers();
}


void Timer(int value)
{

}


void crossProduct(double a[], double b[], double c[])
{
	c[0] = a[1]*b[2] - a[2]*b[1];
	c[1] = a[2]*b[0] - a[0]*b[2];
	c[2] = a[0]*b[1] - a[1]*b[0];
}

void normalize(double a[])
{
	double norm;
	norm = a[0]*a[0] + a[1]*a[1] + a[2]*a[2];
	norm = sqrt(norm);
	a[0] /= norm;
	a[1] /= norm;
	a[2] /= norm;
}

void rotatePoint(double a[], double theta, double p[])
{

	double temp[3];
	temp[0] = p[0];
	temp[1] = p[1];
	temp[2] = p[2];

	temp[0] = -a[2]*p[1] + a[1]*p[2];
	temp[1] = a[2]*p[0] - a[0]*p[2];
	temp[2] = -a[1]*p[0] + a[0]*p[1];

	temp[0] *= sin(theta);
	temp[1] *= sin(theta);
	temp[2] *= sin(theta);

	temp[0] += (1-cos(theta))*(a[0]*a[0]*p[0] + a[0]*a[1]*p[1] + a[0]*a[2]*p[2]);
	temp[1] += (1-cos(theta))*(a[0]*a[1]*p[0] + a[1]*a[1]*p[1] + a[1]*a[2]*p[2]);
	temp[2] += (1-cos(theta))*(a[0]*a[2]*p[0] + a[1]*a[2]*p[1] + a[2]*a[2]*p[2]);

	temp[0] += cos(theta)*p[0];
	temp[1] += cos(theta)*p[1];
	temp[2] += cos(theta)*p[2];

	p[0] = temp[0];
	p[1] = temp[1];
	p[2] = temp[2];

}

void Left()
{
	double speed = 3.1415/8;
	rotatePoint(up,speed,eye);
}

void Right()
{
	double speed = 3.1415/8;
	rotatePoint(up,-speed,eye);
}

void Up()
{
	double speed = 3.1415/8;
	double rot_axis[3];
	double look[3];

	look[0] = -eye[0];
	look[1] = -eye[1];
	look[2] = -eye[2];

	crossProduct(look,up,rot_axis);

	normalize(rot_axis);

	rotatePoint(rot_axis,speed,eye);
	
	rotatePoint(rot_axis,speed,up);

}

void Down()
{
	double speed = 3.1415/8;
	double rot_axis[3];
	double look[3];

	look[0] = -eye[0];
	look[1] = -eye[1];
	look[2] = -eye[2];

	crossProduct(look,up,rot_axis);

	normalize(rot_axis);

	rotatePoint(rot_axis,-speed,eye);
	
	rotatePoint(rot_axis,-speed,up);

}

void IncreaseElbow()
{
	if(ElbowJointAngle > -120)
	{
		ElbowJointAngle = ElbowJointAngle - 5;
	}
}

void DecreaseElbow()
{
	if(ElbowJointAngle <= 0)
	{
		ElbowJointAngle = ElbowJointAngle + 5;
	}
}


void IncreaseWrist()
{
	if(WristJointAngle > -45)
	{
		WristJointAngle = WristJointAngle - 5;
	}
}

void DecreaseWrist()
{
	if(WristJointAngle < 45)
	{
		WristJointAngle = WristJointAngle + 5;
	}
}


void DecreaseFinger()
{
	if(FingerJointAngle > 0)
	{
		FingerJointAngle = FingerJointAngle - 5;
	}
}

void IncreaseFinger()
{
	if(FingerJointAngle < 20)
	{
		FingerJointAngle = FingerJointAngle + 5;
	}
}

void KeyboardEvent(unsigned char key, int x, int y)
{
	switch( key )
	{
		case '1' :  
			IncreaseElbow(); 
			break;
		case '!':  
			DecreaseElbow(); 
			break;
		case '2':  
			IncreaseWrist();
			break;
		case '@':  
			DecreaseWrist();
			break;
		case '3':  
			IncreaseFinger();
			break;
		case '#':  
			DecreaseFinger();
			break;
	}
    
    glutPostRedisplay();
}


void specialKeys(int key, int x, int y)
{
	switch( key )
	{
		case GLUT_KEY_LEFT : 
			Left(); 
			break;
        case GLUT_KEY_RIGHT: 
			Right(); 
			break;
        case GLUT_KEY_UP   :    
			Up(); 
			break;
        case GLUT_KEY_DOWN :  
			Down(); 
			break;
	}
    
    glutPostRedisplay();
}